#pragma config(Sensor, in2,    ppot,                sensorPotentiometer)
#pragma config(Sensor, dgtl3,  touchl,              sensorTouch)
#pragma config(Sensor, dgtl6,  touchh,              sensorTouch)
#pragma config(Sensor, dgtl10, ncoderR,             sensorQuadEncoder)
#pragma config(Sensor, dgtl12, TouchGoal,           sensorTouch)
#pragma config(Motor,  port2,           DriveLF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           DriveRF,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveRB,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           DriveLB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           lift1,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           lift2,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           lift3,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           lift4,         tmotorNormal, openLoop)
#pragma config(Motor,  port10,          Claw,          tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(200)
/*
////////////////////////////////motor ports and explantion not code/////////////////////////////////////////////////////////
// Motor[drivetrain]                                                       \\
//backR = A<---power expander, port 4                                       \\
//backL = B,---power expander, port 5                                        \\
//frontR = port3                                                              \\
//frontL2 = port 2                                                             \\
//because my encoders were not working I had to name them dgtl...               \\
//left encoder = dgtl2 and right encoder = dgtl 4                                \\
//The Finders are just to make looking for the auton and teleop easier            \\
// In theory this program should score 20 points in auton and have a presetn teleop\\
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Lance_Function_Library_v1.0.c" //Funtion Library





void pre_auton()
{
  /*bLCDBacklight = true;
  displayLCDPos(0,0);
  displayNextLCDString("program select");
  redteam = true;
  screenrefresh();
  time1[T1] = 0;
  while (programselecting == true)
  {
    if (nLCDButtons & kLeftButton)
    {
      while (nLCDButtons & kLeftButton)
      {
      }
      if (redteam == true)
      {
        redteam = false;
      }
      else if (redteam == false)
      {
        redteam = true;
      }
      screenrefresh();
    }
    if (nLCDButtons & kCenterButton)
    {
      while (nLCDButtons & kCenterButton)
      {
      }
      programselect = programselect+1;
      if (programselect > totalprogramnumber)
      {
        programselect = 1;
      }
      screenrefresh();
    }
    if (nLCDButtons & kRightButton)
    {
      clearLCDLine(0);
      clearLCDLine(1);
      displayLCDPos(0,0);
      displayNextLCDString("Robot ready");
      wait1Msec(300);
      displayNextLCDString(".");
      wait1Msec(300);
      displayNextLCDString(".");
      wait1Msec(300);
      displayNextLCDString(".");
      wait1Msec(500);
      bLCDBacklight = false;
      programselecting = false;
    }
  }
  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}*/


}

task autonomous()
{
/*  if (programselect == 1);
  {
    if (redteam == true);
    {
      SensorValue[ncoderR] = 0;
      while(SensorValue[ncoderR] <= 100)
      {
           motor[port6] = - 127;
          motor[port7] = -127;
          motor[port8] = -127;
          motor[port9] = -127;
          bLCDBacklight = true;
          LCD();
      }
       allMotorsOff();

      }
    }

    if (programselect == 1);
    {
      if (redteam == false);
      {
         SensorValue[ncoderR] = 0;
        while(SensorValue[ncoderR] >= -100)
        {

          motor[port6] = 127;
          motor[port7] = 127;
          motor[port8] = 127;
          motor[port9] = 127;
             bLCDBacklight = true;
          LCD();

        }
        allMotorsOff();
      }



}
    }// task main


    //lock(low_lock_point); //<-------------------------------------pot code


    //lock(low_lock_point); //<-------------------------------------pot code

*/
}

    //USER CONTROL/////////////////////////////////////////////////////////////////
    task usercontrol()
    {
      //pre_auton();<------


  SensorValue[ncoderR] = 0;
      int goal_lock = 0;  //0 for unlocked, -1 for low gal, 1 for high goal
      //calibration for potentiometer setting the arm
      /*int low_lock_point = SensorValue[in1] + 703; //low goal potentiometer reading
      //sets the low lock point based on the starting potentiometer point
      int high_lock_point = SensorValue[in1] + 1000; //high goal potentiometer reading*/
      //end calibrarion


      while(true)
      {

        //Arm/////////////////////////////////////////////////////////////////////////
        if(vexRT[Btn7L] == 1)//auto button close loop
        {
          goal_lock = -1;   //sets to low lock

        }
        else if(vexRT[Btn7U] == 1) {
          goal_lock = 1;	//sets to high lock
        }
        /*else if(vexRT[Btn7D] == 1) {// descore/ score on low goal...
        goal_lock = -1;
        }*/
        else if(vexRT[Btn7R] == 1) {//same as 7D...
            goal_lock = 2;
        }
        else if (vexRT[Btn7D] ==1){
          goal_lock = 3;
        }


        if(goal_lock == 2){
          lock(eleven_inch_goal); //brings to low lock point
           LCDER("GoalL");
        }
        else if(goal_lock == -1){
          lock(twenty_inch_goal);//moves arm to high lock point
        LCDER("GoalM");
        }
        ///////////////////////////////////////////////////////////////NEW STUFF TAKE THIS OUT IF IT DOESNT WORK


          else if (goal_lock == 3){
          lock(thirty_inch_goal);//moves arm to descore lock point
          LCDER("high");
        }



         else if (goal_lock == 1){
          lock(all_the_way_down);//moves arm to descore lock point
          LCDER("Down");


          /* else if(goal_lock == -1){
          lock(eleven_inch_goal);
          bLCDBacklight = true;
          displayLCDPos(1,10);
          displayNextLCDString("goalM");

        } */}


        //////////////////////////////////////////////////////////////




        //Finder: Arm control
        switch(vexRT[Btn5U] - vexRT[Btn5D]) //manual arm control
        {
        case  1:
          if(SensorValue[touchh] == 0) {
            motor[lift1] = -127;
            motor[lift2] = -127;
            motor[lift3] = -127;
            motor[lift4] = -127;
            goal_lock = 0;  //unlocks arm
            } else {
            motor[lift1] = 0;
            motor[lift2] = 0;
            motor[lift3] = 0;
            motor[lift4] = 0;
          }
          break;

        case -1:
          if(SensorValue[touchl] == 0) {
            motor[lift1] = 127;
            motor[lift2] = 127;
            motor[lift3] = 127;
            motor[lift4] = 127;
            goal_lock = 0;  //unlocks arm
            } else {
            motor[lift1] = 0;
            motor[lift2] = 0;
            motor[lift3] = 0;
            motor[lift4] = 0;
          }
          break;

        case  0:if(goal_lock == 0 ) {
            motor[lift1] = 0;
            motor[lift2] = 0;
            motor[lift3] = 0;
            motor[lift4] = 0;
            break;
          }//switch
        }

        // Finder: Claw
        switch(vexRT[Btn6D] - vexRT[Btn6U])
        {
        case  1:motor[Claw] = -127; //Close
          break;
        case -1:motor[Claw] = 127; //Open
          break;
        case  0:motor[Claw] = 0; //Do nothing
          break;
        }
        // Finder :Drive Train
        if(vexRT[Ch2] < 30 && vexRT[Ch2]> -30 && vexRT[Ch3] < 30 && vexRT[Ch3]> -30)
        {
          motor[DriveLF] = 0;
          motor[DriveRF] = 0;
          motor[DriveRB] = 0;
          motor[DriveLB] = 0;

        }
        {
          motor[DriveLF] = vexRT[Ch2];//left frotn motor
          motor[DriveRF] = vexRT[Ch3];//front right motor
          motor[DriveRB] = vexRT[Ch3];//right back motors
          motor[DriveLB] = vexRT[Ch2];//Back left motor
        }







        // Shuts all motors down


    if(Btn8D == 1)
    {
     while(SensorValue[ncoderR] < 40 && touchh == 1 && Btn8R == 1)


     {
     motor[port8] = -127;
     motor[port7] = -127;

     }

    }



LCD();
      }//while


    }//taskusercontrol










    //////////////////////////////////////////////////////////////////////////////////////////////
    /*eleven_inch_goal= 347;
    twenty_inch_goal = 3000; // THese are the encoder values for the arm. CHange them if the arm is not going to the right pos.
    thirty_inch_goal = 4000;//<<<<---------------------------------------------------run debugger in tele op and see what this value is,
    all_the_way_down = 0; //ANother preset but we do not need it.
    */

    //arm_grounded += 250;

    //main code-to be placed in pre-auton section:
